#include "LSM303compass.h"

LSM303compass::LSM303compass()
{
	// These are just some values for a particular unit, it is recommended that
	// a calibration be done for your particular unit.

        m_max.x=320.00;	m_max.y=512.00;	m_max.z=374.00;
	m_min.x=-654.00;m_min.y=-455.00;m_min.z=-414.00;


	old_yaw=-10000000;
}

// Public Methods //////////////////////////////////////////////////////////////

// Turns on the LSM303DLH's accelerometer and magnetometers and places them in normal
// mode.
void LSM303compass::enable()
{
	//Enable Accelerometer
	Wire.beginTransmission(ACC_ADDRESS);
	Wire.send(CTRL_REG1_A);
	//0x27 = 0b00100111
	// Normal power mode, all axes enabled
	Wire.send(0x27); 
	//0x87 = 0b10000111
	// low power mode (2Hz), all axes enabled
	//Wire.send(0x87); 

	Wire.endTransmission();
  
	//Enable Magnetometer
	Wire.beginTransmission(MAG_ADDRESS);

	//0x00 = 0b00000000
	// Continuous conversion mode
	Wire.send(MR_REG_M);
	Wire.send(0x00);

	//30Hz update
	//0x14 = 0b00010100
	Wire.send(CRA_REG_M);
	Wire.send(0x18);


	//0x01 = 0b00000001
	// Discrete conversion mode (sleep until read)
	//Wire.send(0x01);

	Wire.endTransmission();
}

// Reads all 6 channels of the LSM303DLH and stores them in the object variables
void LSM303compass::read(void)
{

	//read accelerometer
	Wire.beginTransmission(ACC_ADDRESS);
	// assert the MSB of the address to get the accelerometer 
	// to do slave-transmit subaddress updating.
	Wire.send(OUT_X_L_A | (1 << 7)); 
	Wire.endTransmission();
	Wire.requestFrom(ACC_ADDRESS,6);

	while (Wire.available() < 6);
	
	uint8_t xla = Wire.receive();
	uint8_t xha = Wire.receive();
	uint8_t yla = Wire.receive();
	uint8_t yha = Wire.receive();
	uint8_t zla = Wire.receive();
	uint8_t zha = Wire.receive();

	a.x = (xha << 8 | xla) >> 4;
	a.y = (yha << 8 | yla) >> 4;
	a.z = (zha << 8 | zla) >> 4;


	
	//read magnetometer
	Wire.beginTransmission(MAG_ADDRESS);
	Wire.send(OUT_X_H_M);
	Wire.endTransmission();
	Wire.requestFrom(MAG_ADDRESS,6);

	while (Wire.available() < 6);

	uint8_t xhm = Wire.receive();
	uint8_t xlm = Wire.receive();
	uint8_t yhm = Wire.receive();
	uint8_t ylm = Wire.receive();
	uint8_t zhm = Wire.receive();
	uint8_t zlm = Wire.receive();

	m.x = (xhm << 8 | xlm);
	m.y = (yhm << 8 | ylm);
	m.z = (zhm << 8 | zlm);

	// shift and scale
	m.x = (m.x - m_min.x) / (m_max.x - m_min.x) * 2 - 1.0;
	m.y = (m.y - m_min.y) / (m_max.y - m_min.y) * 2 - 1.0;
	m.z = (m.z - m_min.z) / (m_max.z - m_min.z) * 2 - 1.0;

	attitude();
}


float LSM303compass::lowPassFilter(float value,float old_value,float alpha)
{
	return old_value+alpha*value-alpha*old_value;
}

/* 
void LSM303compass::readFilter(float alpha)
{
	read();

	if (old_m.x==-10000000) {
		old_m.x=m.x;
		old_m.y=m.y;
		old_m.z=m.z;

		old_a.x=a.x;
		old_a.y=a.y;
		old_a.z=a.z;
	}
	a.x=lowPassFilter(a.x,old_a.x,alpha);
	a.y=lowPassFilter(a.y,old_a.y,alpha);	
	a.z=lowPassFilter(a.z,old_a.z,alpha);		

	m.x=lowPassFilter(m.x,old_m.x,alpha);
	m.y=lowPassFilter(m.y,old_m.y,alpha);	
	m.z=lowPassFilter(m.z,old_m.z,alpha);		

	old_m.x=m.x;
	old_m.y=m.y;
	old_m.z=m.z;

	old_a.x=a.x;
	old_a.y=a.y;
	old_a.z=a.z;

	attitude();
}*/


void LSM303compass::readFilter(float alpha)
{
	read();
	attitude();
	if (old_yaw==-10000000) {
		old_yaw=yaw;
		old_pitch=pitch;
		old_roll=roll;
	}

	yaw=lowPassFilter(yaw,old_yaw,alpha);
	pitch=lowPassFilter(pitch,old_pitch,alpha);	
	roll=lowPassFilter(roll,old_roll,alpha);		

	old_yaw=yaw;
	old_pitch=pitch;
	old_roll=roll;
}


